import matplotlib.pyplot as plt
from matplotlib.pyplot import MultipleLocator
import numpy as np
import math
from matplotlib import cm
import time
import random
import copy
import sys,os

def wait_key():
    ''' Wait for a key press on the console and return it. '''
    result = None
    if os.name == 'nt':
        import msvcrt
        result = msvcrt.getch()
    else:
        import termios
        fd = sys.stdin.fileno()
 
        oldterm = termios.tcgetattr(fd)
        newattr = termios.tcgetattr(fd)
        newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO
        termios.tcsetattr(fd, termios.TCSANOW, newattr)
 
        try:
            result = sys.stdin.read(1)
        except IOError:
            pass
        finally:
            termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
 
    return result

# 栅格类
class Grid:
    def __init__(self,width,height,gridSize,robot_location,target_location,obs,draw):
    # 初始化栅格
        self.Width = width  # 宽度的栅格数
        self.Height = height
        self.gridSize = gridSize    # 栅格的大小
        self.grid = np.zeros(((int)(self.Height), (int)(self.Width)))   # 栅格地图（有障碍物置1)
        self.obs = obs     # 障碍物坐标
        self.robotLoc = robot_location  # 机器人起始在栅格的位置
        self.targetLoc = target_location    # 目标点位
        self.ENABLE_DRAW = draw     # 是否允许绘制动图
        self.creatGrid()    # 创建栅格空间
        self.writeObstacle()

    def creatGrid(self):
        # 创建栅格空间
        self.field_array = self.grid.copy()
        plt.figure("map")
        plt.axis('scaled')  # 横纵坐标大小刻度一致     
        axis = plt.gca()    # 获取坐标轴的点
        xAxis = np.arange(0,(self.Width+1)*self.gridSize,self.gridSize) # 设置栅格
        yAxis = np.arange(0,(self.Height+1)*self.gridSize,self.gridSize)
        axis.set_xticks(xAxis)
        axis.set_yticks(yAxis)
        axis.tick_params(axis = 'both', direction = 'in')
        plt.grid(True)  # 绘制栅格

        # 绘制机器人坐标 和 终点坐标
        self.drawPoint(self.robotLoc,markersize = 6, marker = 'o', markerfacecolor = 'red', markeredgecolor = 'red')
        self.drawPoint(self.targetLoc,markersize = 6, marker = 'v', markerfacecolor = 'green', markeredgecolor = 'green')
        
    def writeObstacle(self):
        # 将障碍物 写进 栅格（相同的坐标置 1）
        for item in list(self.obs):
            self.grid[item[0],item[1]] = 1;
        # 绘制障碍物
        for item in list(self.obs):
            self.drawPoint(item, markersize = 4,marker = 's', markerfacecolor = 'black', markeredgecolor = 'black')
        if self.ENABLE_DRAW:
            # plt.show()
            plt.ioff()
            plt.gcf().show()

    def drawPoint(self,loction,markersize = 4,marker = 's',markerfacecolor = 'black',markeredgecolor = 'black'):
        # 将点绘制进栅格（因为栅格用刻度表示，所以绘制点时需要加上偏置）
        plt.plot(loction[0]*self.gridSize + self.gridSize/2,loction[1]*self.gridSize + self.gridSize/2,
                 linestyle = '',
                 markersize = markersize,
                 marker = marker,
                 markerfacecolor = markerfacecolor,
                 markeredgecolor = markeredgecolor)

def creatObstacle(Height,Width):
    # 创建障碍物 
    obs = []

        # 含有边界的栅格
    obs1 = [(0,a) for a in range((int)(Height))]
    obs2 = [((int)(Width) - 1,a) for a in range((int)(Height))]
    obs3 = [(a,0) for a in range((int)(Width))]
    obs4 = [(a,(int)(Height) - 1) for a in range((int)(Width))]
    # obs5 = [(13,a) for a in range(20)]
    # obs6 = [(26,a + 20) for a in range(20)]
    # obs7 = [(a + 26,20) for a in range(8)]
    # obs8 = [(34,6+a) for a in range(15)]
    
    obs5 = [(a,b) for a in range(10,14,1) for b in range(10,14,1)]
    obs6 = [(a,b) for a in range(18,22,1) for b in range(10,14,1)]
    obs7 = [(a,b) for a in range(18,26,1) for b in range(18,22,1)]
    obs8 = [(a,b) for a in range(26,30,1) for b in range(26,30,1)]
    obs9 = [(a,b) for a in range(18,22,1) for b in range(26,30,1)]
    
    '''
    obs5 = [(a,b) for a in range(5,7,1) for b in range(5,7,1)]
    obs6 = [(a,b) for a in range(9,11,1) for b in range(5,7,1)]
    obs7 = [(a,b) for a in range(9,13,1) for b in range(9,11,1)]
    obs8 = [(a,b) for a in range(13,15,1) for b in range(13,15,1)]
    obs9 = [(a,b) for a in range(9,11,1) for b in range(13,15,1)]
    '''
    # 含有局部极小值的障碍物
    obs10 = [(8,b) for b in range(0,8,1)]
    obs11 = [(a,8) for a in range(1,9,1)]
    
    # 上次Astar使用障碍物 
    '''
    obs1 = [(0,a) for a in range((int)(self.Height))]
    obs2 = [((int)(self.Width) - 1,a) for a in range((int)(self.Height))]
    obs3 = [(a,0) for a in range((int)(self.Width))]
    obs4 = [(a,(int)(self.Height) - 1) for a in range((int)(self.Width))]
    obs5 = [(13,a) for a in range(20)]
    obs6 = [(26,a + 20) for a in range(20)]
    obs7 = [(a + 26,20) for a in range(8)]
    obs8 = [(34,6+a) for a in range(15)]
    '''

    # 创建障碍物
    obs = obs + obs5 + obs6  + obs7 + obs8 + obs9 + obs10 + obs11#  + obs1 + obs2 + obs3 + obs4 
    
    return obs

# 遗传算法
class GeneticAlgorithm:
    
    # 初始化
    def __init__(self,grid:Grid,popuQuantity,numGen):
        self.Grid = grid
        self.grid = grid.grid
        self.population = None
        self.banPos = []    # 起始种群的个体创建时的禁止搜索的栅格
        self.startPopNum = popuQuantity # 起始种群个体数量
        self.numGen = numGen
        return

    # 路径规划
    def planning(self):
        self.creatPopulation()  # 生成初始种群
        numGen,disAverGen,disMinGen = [],[],[]
        
        # 迭代次数
        
        for i in range(self.numGen):
            print("***********************************************************")
            print('time',i)
            print('NumPopu',len(self.population))
            t1 = time.time()
            chPopu = self.choose(self.population,60)  # 存活下来的个体
            t2 = time.time()
            print("T choose: ",t2 - t1)
            vaPopu = self.variation(chPopu,200) # 变异的个体
            t3 = time.time()
            print("T variation: ",t3 - t2)
            crPopu = self.crossover(chPopu,200) # 交叉的个体
            t4 = time.time()
            print("T crossover: ",t4 - t3)
            temp = chPopu + crPopu + vaPopu   # 总个体
            self.population = self.choose(temp,100)
            self.population.sort(key = lambda a: a[1],reverse = False)
            disAverGen.append(self.calAverageDis(self.population))  # 每一代的平均路径
            numGen.append(i)    # 代 数
            disMinGen.append(self.population[0][1]) # 每一代最短距离
            t5 = time.time()
           
            print("T other: ",t5 - t4)

        # 绘制
        for item in self.population:    
            #self.drawPath(item[0])
            self.drawPath(self.population[0][0])
        path = self.population[0][0]
        plt.figure("gengeticAlgo...")
        plt.plot(numGen,disAverGen)
        plt.plot(numGen,disMinGen)
        self.drawPath(path)
    
    # 生成初始种群
    def creatPopulation(self):
   
        self.population = []
        # 初始种群的数量
        for i in range(self.startPopNum):
            current = self.Grid.robotLoc    # 
            self.banPos = []
            list = [self.Grid.robotLoc] # 路径列表
            distance = 0
            while current != self.Grid.targetLoc:
                nextPos,dist = self.getNextPos(current,self.Grid.targetLoc) # 寻找下一点
                if len(self.banPos) >= 2:   # 当禁止搜索的点大于2时
                    del self.banPos[0]  
                self.banPos.append(nextPos) # 将刚刚搜索过的点作为禁止搜索的点

                distance = distance + dist  # 距离增加
                current = nextPos   # 当前点位位移
                list.append(current)    # 路径列表，存放的是点位
            self.population.append([list,distance])  # 种群个体增加     

    # 为两点寻找下一点位，依据：离目标近的点的概率大
    def getNextPos(self,current,target):

        motion = self.get_motion_model()
        probability = []
        dis = []
        nextPoses = [(item[0] + current[0],item[1] + current[1],item[2]) for item in motion]

        # 初步筛选点位，下一点位列表中删除障碍物点，边界点
        i = 0
        while i < len(nextPoses):   # 
            item = (nextPoses[i][0],nextPoses[i][1])
            if 0 <= item[0] < self.Grid.Width and 0 <= item[1] < self.Grid.Height and item not in self.Grid.obs and item not in self.banPos:
                i = i + 1
                continue
            nextPoses.pop(i)
        
        # pro_distribute = [0,0.04,0.095,0.15,0.225,0.3,0.5,0.7,1] # 概率分布
        pro_distribute = [0,0.02,0.08,0.12,0.19,0.25,0.4,0.6,1] # 概率分布
        # 生成一个选择的数，在（0，下一点位列表的长度）,下一点就由这个数决定
        a = pro_distribute[len(nextPoses)] * random.random() # 在随机选一个数

        index = 0
        # 计算这个数对应的索引
        for i in range(len(pro_distribute)-1):
            if pro_distribute[i] < a <= pro_distribute[i+1]:
                index = i
                break
        
        # 
        i = 0
        for nextPos in nextPoses: 
            dist0 = self.get_distance(nextPos,target)
            dis.append([dist0,(nextPos[0],nextPos[1]),nextPos[2]]) 
            i = i + 1

        dis.sort(key=lambda dis: dis[0],reverse = True) # 对距离降序排列，距离大的概率小
        usless,nextPos,stepDistance = dis[index]  
        return nextPos,stepDistance 


    def get_distance(self,pos1,pos2): 
        return math.sqrt(math.pow(pos1[0]-pos2[0],2) + math.pow(pos1[1]-pos2[1],2))

    def generate_continuous_path(self,pos1,pos2):
        obs = self.Grid.obs
        current = pos1
        self.banPos = []
        subPopu = [pos1]
        distance = 0
        while current != pos2:
            nextPos,dist = self.getNextPos(current,pos2)
            if len(self.banPos) >= 3:
                self.banPos.pop(0)
            self.banPos.append(nextPos)
            distance = distance + dist  
            current = nextPos
            subPopu.append(current)
        # self.drawPath(subPopu)
        return subPopu


    # 交叉
    def crossover(self,population0,times):
        newPopu = []
        population = copy.deepcopy(population0)
        for i in range(int(times/2)):
            a = random.randint(0,len(population) - 1)
            b = random.randint(0,len(population) - 1)
            a_index = random.randint(0,len(population[a][0]) - 2)
            currentPos = population[a][0][a_index]
            while currentPos not in population[b][0]:
                a_index = a_index + 1
                currentPos = population[a][0][a_index]
                if currentPos == self.Grid.targetLoc:
                    break
                

            if currentPos == self.Grid.targetLoc:
                 continue
            
            a_range0 = population[a][0][0:a_index+1:1]
            a_range1 = population[a][0][a_index + 1::1]

            b_index = 0
            while currentPos != population[b][0][b_index]:
                b_index = b_index + 1
            b_range0 = population[b][0][0:b_index+1:1]
            b_range1 = population[b][0][b_index+1::1]
            
            new0 = a_range0 + b_range1
            new1 = b_range0 + a_range1
            
            newPopu.append([new0,self.calPathDis(new0)])
            newPopu.append([new1,self.calPathDis(new1)])
        return newPopu
    
    # 变异
    def variation(self,population0,times):
        posVar = []
        population = copy.deepcopy(population0)
        self.variationLen = [3,5,8,10]  # 变异长度
        row,rank = len(population),len(min(population,key = lambda population:len(population[0]))[0])
        newPopu0 = []
        #self.variationLen[1]
        
        for i in range(int(times)):
            x = random.randint(0,row - 1)
            y = random.randint(0,rank - 1)
            lenV = random.randint(0,rank - y)
            posVar.append((x,y))
            pop = population[x][0]
            a = []
            if len(pop) > y + lenV + 1: # 变异长度
                subPopu = self.generate_continuous_path(pop[y],(pop[y + lenV]))       

                a0 = pop[0:y:1]
                a1 = pop[y + lenV + 1::1]
                a = a0 + subPopu + a1
            if a == []:
                continue
            newPopu0.append([a,self.calPathDis(a)])
           
        
        return newPopu0

    

    # 选择
    def choose(self,population,num):
        
        fit,fitAll = self.cal_fit(population)
        popu = []

        # 第一种：轮盘赌
        for i in range(len(fit)):
            temp = fit[i][1]/fitAll
            if temp < 0:
                temp = 0
            p = random.random()
            if temp * num >= p:
                popu.append(fit[i][0])
        '''
        # 第二种：直接选择优势个体
        fit.sort(key = lambda a: a[1],reverse = True)
        for i in range(num):
            popu.append(fit[i][0])
        '''
        return  popu

    def cal_fit(self,population):
        fit,fit1,fit2 = [],[],[]
        AllDis = 0
        
        for [unin,dis] in population:
            fit1.append(1/dis)
            AllDis = AllDis + 1/dis
            costAll = 1/len(population)
            for i in range(len(unin)-2):
                a1 = self.get_distance(unin[i],unin[i+1])
                a2 = self.get_distance(unin[i+1],unin[i+2])
                a3 = self.get_distance(unin[i],unin[i+2])

                if a1 != 0 and a2 != 0:
                    cos = (math.pow(a1,2) + math.pow(a2,2) - math.pow(a3,2))/(2 * a2 * a1)
                else:
                    cos = 1.5/math.pow(len(population),2)
                cost = 0
                if cos < 0.5:
                    cost = 0.5/math.pow(len(population),2)
                if cos < 0:
                    cost = 1/math.pow(len(population),2)

                costAll = costAll - cost

            fit2.append(costAll)
        fitAll = 0
        for i in range(len(fit1)):
            fit1[i] = fit1[i]/AllDis
            fit.append([population[i] , 0.95*fit1[i] + 0.05*fit2[i]])
            fitAll = fitAll +  0.95*fit1[i] + 0.05*fit2[i]
        return  fit,fitAll

    def calAverageDis(self,population):
        Dis = 0
        for [item,dist] in population:
            Dis = Dis + dist
        
        return Dis/len(population)

    def calPathDis(self,path):
        distance = 0
        for i in range(len(path)-1):
            step = 1
            if max(abs(path[i + 1][0] - path[i][0]),abs(path[i + 1][1] - path[i][1])) > 1:
                print('error:not continuous')
                pass
            if min(abs(path[i + 1][0] - path[i][0]),abs(path[i + 1][1] - path[i][1])) == 1 :
                step = math.sqrt(2)
            distance = distance + step
        return distance

    def drawPath(self,path):
    # 计算最终的路径，根据 节点的上一节点 进行遍历，寻找到第一个节点
        rx,ry = [],[]
        for item in path:
            rx.append(item[0]*self.Grid.gridSize + self.Grid.gridSize/2)
            ry.append(item[1]*self.Grid.gridSize + self.Grid.gridSize/2)
        plt.figure("map")
        plt.plot(rx,ry)
        plt.ioff()
        plt.gcf().show()
        return

    def drawPopu(self,popu):
        return
    @staticmethod
    def get_motion_model():
        # dx, dy, cost
        motion = [[1, 0, 1],
                  [0, 1, 1],
                  [-1, 0, 1],
                  [0, -1, 1],
                  [-1, -1, math.sqrt(2)],
                  [-1, 1, math.sqrt(2)],
                  [1, -1, math.sqrt(2)],
                  [1, 1, math.sqrt(2)]]
        return motion

def main():
    start_pos = (5,5)
    end_pos = (35,35)
    grid_width = 40
    grid_height = 40
    min_grid = 5
    obs = creatObstacle(40,40)
    grid = Grid(grid_width,grid_height,min_grid,start_pos,end_pos,obs,True)
    geneticAlgorithm = GeneticAlgorithm(grid,100,200)
    geneticAlgorithm.planning()
   
    plt.show()

    return
    
if __name__ == '__main__':
    main()